function [cost_val] = CalcSensingCost(i,j,m,n,weather,aircraft,time,alt,cost,Jv)
%*=+--+=#=+--      UAS Severe Weather Simulation Softeware        --+=#=+--+=#*%
%          Copyright (C) 2013 Regents of the University of Colorado.           %
%                             All Rights Reserved.                             %
%                                                                              %
%    This program is free software: you can redistribute it and/or modify      %
%    it under the terms of the GNU General Public License version 2 as         %
%    published by the Free Software Foundation.                                %
%                                                                              %
%    This program is distributed in the hope that it will be useful,           %
%    but WITHOUT ANY WARRANTY; without even the implied warranty of            %
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             %
%    GNU General Public License for more details.                              %
%                                                                              %
%    You should have received a copy of the GNU General Public License         %
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.     %
%                                                                              %
%           Jack Elston                                                        %
%           elstonj@colorado.edu                                               %
%                                                                              %
%*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*%

  frame = weather.frame;
  shift_x = floor((frame.u * (time - weather.t0))/weather.xs);
  shift_y = floor((frame.v * (time - weather.t0))/weather.ys);

  %rain_weight = 5*10^2;
  vario_weight = 20^3;

  d.x = i - m;
  d.y = j - n;

  %vector = weather(time, [(i-1)*weather.xs+weather.x0,(j-1)*weather.ys+weather.y0,alt]);
  vector2 = weather(time, [(m-1)*weather.xs+weather.x0,(n-1)*weather.ys+weather.y0,alt]);
  %c.x = (vector(1)+vector2(1))/2;
  %c.y = (vector(2)+vector2(2))/2;
  c.x = vector2(1);
  c.y = vector2(2);
  c.z = vector2(3);
  rain = vector2(4);
  %hail = vector2(5);

  magnitude = aircraft.velocity;  %FIXME and make work with acutal UA speed
  theta = atan2(d.y,d.x);
  v.c = magnitude * [cos(theta), sin(theta)];
  v.r = v.c + [c.x c.y];
  v.c2 = v.c(1)^2 + v.c(2)^2;
  c2 = c.x^2 + c.y^2;
  delta = v.c2*(d.x^2+d.y^2) - (c.x*d.y-c.y*d.x)^2;

  o = floor(i/ceil(weather.xnum / size(Jv,2)))+1;
  p = size(Jv,1)-1-floor(j/ceil(weather.ynum / size(Jv,1)))+1;

  if (delta < 0)
    cost_val = Inf;
  else
    if (v.c2-c2) == 0
      cost_val = (d.x^2+d.y^2)/(2*(c.x*d.x+c.y*d.y));
    else
      cost_val = (-(c.x*d.x+c.y*d.y) + sqrt(delta))/(v.c2-c2);
    end
    if cost_val < 0.0
      cost_val = Inf;
    else
      cost_val = cost_val - vario_weight * Jv(p,o) + cost(m-shift_x,n-shift_y);
    end
  end
